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SUMMARY 


This thesis investigates the measurement and control of the 
end-effector position of a large two-link flexible manipulator. The 
system implementation is described and an initial algorithm for 

static end-point positioning is discussed. 

Most existing robots are controlled through independent joint 
controllers, while the end-effector position is estimated from the 
joint positions using a kinematic relation. End-point position 
feedback can be used to compensate for uncertainty and structural 
deflections. Such feedback is especially important for flexible 

robots. 

Computer vision is utilized to obtain end-point position 
measurements. A look-and-move control structure alleviates the 
disadvantages of the slow and variable computer vision sampling 
frequency. This control structure consists of an inner joint-based 

loop and an outer vision-based loop. 

A static positioning algorithm was implemented and 
experimentally verified. This algorithm utilizes the manipulator 
Jacobian to transform a tip position error to a joint error. The joint 
error is then used to give a new reference input to the joint 
controller. The convergence of the algorithm is demonstrated 
experimentally under payload variation. 



A Landmark Tracking System [Dickerson, et al 1990] is used 
for vision-based end-point measurements. This system was modified 
and tested. A real-time control system was implemented on a PC and 
interfaced with the vision system and the robot. 

Future extensions of this work are discussed. 
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CHAPTER I 


INTRODUCTION 


1.1. Motivation 

This thesis investigates the measurement and control of the 
end-effector position of a large two-link flexible manipulator and 
describes the initial system implementation. 

Most existing robots are controlled through independent joint 
controllers, while the end-effector position is estimated from the 
joint positions using a kinematic relation. If those robots have a 
stiff structure, structural deflections can be neglected. 
Disadvantages of this concept are heavy weight, need for powerful 
actuators, slow operation speed, limited work space and resulting 
low accuracy. 

End-point position feedback can be used to compensate for 
uncertainty and structural deflections and to increase the 
positioning accuracy. Such feedback is especially important for 
flexible robots. 

Various methods have been published that claim good results 
for end-point control of flexible robots. But only some of these 
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researchers prove their methods with a direct measurement of end- 
point position, while others use an estimation derived from joint 
and flexible states. Similarly not all end-point control methods use 
an end-point position feedback. Lack of readily available sensors for 
a direct measurement encourage this approach. From those deficits 
comes the motivation to develop a system for end-point position 
measurement and control. 

Existing robot applications use computer vision for inspection 
and guidance in partially unstructured environments. The acceptance 
and availability of vision in robotics suggests to use vision also for 
the position feedback. An inertial measurement (accelerometer) 
could alternatively be used. Drawbacks of this method are the error 
resulting from the integration of the original signal and the lack of 
an absolute measurement. This argument motivates the use of 
computer vision for position measurements and feedback control. 


1.2. Flexible Manipulators 

Robots derived from a concept that allows and considers 
structural deflections are called flexible robots. The development of 
flexible robots has several motivations: 1. applications in space 
which dictate dramatically reduced weight, 2. mobile robots for 
construction work, maintenance of electricity and communication 
cables etc. which demand reduced weight, reduced power 
requirements for actuation and increased work-space size, 3. 
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handling of heavy parts which requires increased payload-to- 
weight-ratios, 4. inspection and manufacturing of large structures 
like airplane bodies etc. which demand increased work-space, 5. 
robots for high speed operation which require reduced inertias. 
Existing examples of flexible robots are the space shuttle arm and a 
variety of robots in research laboratories all over the world, ranging 
from very flexible study objects to applied prototypes. 

The structural flexibility introduces characteristics that have 
to be considered when designing the control for a flexible robot. 
First, the links of the robot are a continuous system exhibiting an 
infinite number of flexible modes. Neglecting higher frequencies and 
modelling the system with a finite number of modes causes some 
uncertainty. Second, the dynamic equations are nonlinear, with the 
nonlinear terms being centrifugal and Coriolis forces. Linearizing 
those equations for an operating point causes more uncertainty, 
especially for high speed motion and for operation far away from the 
operating point. Third, the end-point control is a non-colocated 
problem. That is, the robot is controlled based on measurements at 
one end of the kinematic chain by actuators at the other end. 
According to Cannon and Schmitz (1984), direct feedback of the end- 
point position tends to make the system unstable or conditionally 
stable. Therefore the control has to be designed carefully. Fourth, 
the transfer function from actuators to tip position is non-minimum 
phase. Mathematically this is described by right half plane zeros, 
meaning physically that the tip initially moves in the opposite 
direction of the motor input, i.e. in the "wrong" direction. 
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1.3. Review of Previous Work 

End-point control of flexible manipulators 
This section discusses publications presenting the motion 
control of flexible robots. Some of the described methods use end- 
point feedback with vision. However, the vision is treated as a 
black-box providing the position of the end-point, the special 
characteristics of vision are not considered. 

Optimal control design with pure feedback of tip-position, 
joint rate and strain was introduced by Cannon and Schmitz (1984). 
Those researchers investigated a one-link flexible robot, a one 
meter long arm that moved in the horizontal plane. This arm was 
modelled with the assumed modes method, linearized for an 
operating point and parameters experimentally identified. The 
controller was digitally implemented with a sampling frequency of 
50 Hz. This is 100 times faster than the natural cantilever 
frequency of 0.5 Hz. Feedback of the end-point position was provided 
from an analog sensor. The authors reported that the closed-loop 
system remained conditionally stable and the feedback of joint rate 
and strain was helpful in achieving good closed-loop performance. 
The presented step responses show two characteristics: First, the 
speed of response is faster than the first natural period, but 
ultimately limited by the wave-propagation delay of the flexible 
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beam. Second, the response is that of a non-minimum phase system, 
that is the tip moves initially in the wrong direction. 

The optimal control concept was extended to end-point 
tracking by Oakley and Cannon (1989) and Oakley and Barratt (1990). 

In the first paper an LQ regulator and a nonlinear estimator were 
used to control a two-link robot with a rigid upper arm and a 
flexible lower arm. In the second paper a two-link robot with two 
flexible arms was controlled by an augmented LOG controller with 
FIR filter. In both cases the controller worked synchronously with a 
vision system for end-point position measurements. A trajectory 
was specified in Cartesian space for end-point tracking. Desired 
rigid states for reference commands were computed using rigid 
inverse kinematics, while the desired flexible states were set to 
zero. Measured end-point position data shows approximate tracking 
with some over-shoot and the second controller following the 

desired trajectory more closely. 

Two feedback loops, an inner loop to control motor position 

and an outer loop to control tip position, were suggested by Rattan, 
Feliu and Brown (1990). The design procedure was described for a 
simplified one-link flexible robot, a structure consisting of a 
"mass-less" link (music wire) with lumped mass. A camera tracking 
a LED mounted at the tip of the arm was used for end-point 
measurements. The inner loop is used to remove the effect of 
friction in the joint. The outer loop design considers only the arm 
dynamics, since the dynamics of the closed inner loop are fast 
compared with the arm dynamics. The outer loop is composed of a 
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model based portion and a servo portion. Feedforward terms are used 
in the servo portion to follow the reference commands. 

A Cartesian control schema was investigated by Lee, 
Kawamura, Miazaki and Arimoto (1990). The transposed manipulator 
Jacobian relates a force at the robot tip (Cartesian space) to joint 
torques Qoint space). This was used by the authors to transform a 
proportional Cartesian control action to an equivalent joint control 
action. Since a proportional control alone is not sufficient for 
closed-loop stability, a joint rate feedback is added. To improve 
closed-loop performance also an additional strain feedback was 
added. This concept was verified using a two-link robot, but only the 
response to step changes in the reference position was discussed 

end-point tracking was not attempted. 

An inverse dynamic method for end-point tracking is presented 
by Kwon and Book (1990). The inverse dynamic method is a 
feedforward control and does not use end-point feedback. 
Feedforward control does not affect the closed-loop poles and 
therefore has the advantage of not making the closed-loop system 
unstable. But feedforward control requires a good dynamic model, as 
any open-loop control does. The feedforward control computes the 
required torque for tracking the end-point from the inverse 
dynamics. Therefore it is necessary to specify desired states along 
the desired trajectory. This presents the problem that desired 
flexible states have to be known for a flexible robot. Kwon and Book 
present a method to compute the desired flexible coordinates, 
accepting a non-causal solution. This is a result of the non-minimum 
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phase characteristics of the flexible system, i.e. the right half plane 
zeros, which become poles of the inverse dynamics, would make a 
causal inverse dynamics solution unstable, if a causal solution is 
accepted. 


Vision in robot control s ystems; 

This paragraph describes some characteristics of vision in 
feedback control. 

Visual servo control structures were classified by Sanderson 
and Weiss (1983). Fig. 1.1. shows this classification in a diagram. 

Two feedback representations, position-based and image- 
based, were suggested. The feedback signal in the first case is a 
position, e.g. the position of a target relative to the end-effector. 
Tha feedback signal in the second case is an image feature, e.g. an 
area, a length or an angle. The second representation has the 
advantage of reduced on-line computation, the desired trajectory is 
specified in desired image features and can be taught off-line. The 
position-based feedback has the advantage of appearing more natural 
to feedback control and desired positions can be computed easily. 

In a closed-loop joint control structure (Look-and-Move) a 
inner feedback loop is closed around the joints and an outer feedback 
loop is closed from the measured robot position to the desired robot 
position. The outer control loop sends a joint reference command to 
the inner control loop. An open-loop joint control structure utilizes 
only feedback of the measured robot position. An advantage of the 
first structure is that different sampling rates can be used in the 
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two control loops and the joint loop can operate with a much faster 
sampling rate than the "slow" vision. It might also be necessary to 
close the inner loop for stability reasons, [Cannon and Schmitz, 
1984]. 

Timing between joint and vision measurements was used for a 
further classification of the look-and-move structure. Vision and 
joint feedback are completely independent for a static look-and- 
move structure, i.e. the vision based control updates the joint 
reference input only after the robot came to a complete stop. In a 
dynamic look-and-move structure the vision based control updates 
the joint reference input during motion. 

Most existing systems use variations of the look-and-move 
structure. Coulon and Nougaret (1983) performed a dynamic analysis 
of a TV camera for feedback control and demonstrated visual control 
of a X-Y-plotter. Feddema and Mitchell (1989) used an image-based 
look-and-move structure with a feature-based trajectory generator 
to track a moving target. Their joint controller received velocity 
commands from the trajectory generator that matched velocity 
boundary conditions at vision updates. 

These applications used TV cameras and specialized computer 
vision equipment. Existing sensors for vision were discussed by 
Dunbar (1986). Dunbar pointed out some handicaps of the existing TV 
standard for computer vision applications. An interesting new 
approach to computer vision was chosen by Dickerson, et al (1990). 
Those researchers designed a landmark tracking system (LTS), 
consisting of illumination, optics, CCD detector, a processor and 
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software. Based on the experimental works using both the off-the- 
shelf vision system (breadboard configuration) and the LTS, K.-M. Lee 
et al (1991) identified several problems associated with the 
breadboard configuration and the LTS for machine vision 
applications. The LTS is currently used in two applications related 
to this thesis: Nam and Dickerson (1991) use vision and 

accelerometer to obtain position estimates with increased 
performance and bandwidth. Smith (1991) applies an integrated 
vision system, based on the electronic framework of the LTS, to 
end-point control of a three link in-parallel manipulator. 


1.4. Chosen Approach 


A look-and-move control structure, consisting of an inner 
joint-based loop and an outer vision-based loop was chosen for 
several reasons: First this structure alleviates the disadvantages of 
the intrinsically slow and varying computer vision sampling 
frequency. The two control loops operate on different sampling rates 
with the inner joint-based loop being much "faster" than the outer 
vision-based loop. Second, joint feedback is suggested for stability 
of the closed loop system [Cannon and Schmitz, 1984]. Third this 
structure is easily modified for future extensions. 

The Landmark Tracking System (LTS) was used for vision. This 
independent system processes all time consuming vision 
computations on the integrated processor. The output of the system 



10 


is a landmark position. For this reason positions were chosen as 
visual feedback representation. 

A static positioning algorithm was chosen for the first 
implementation. This algorithm has the advantage of decreased 
complexity, especially the stability of the closed-loop system is 
less crucial, while addressing most implementation issues. The 
algorithm utilizes the manipulator Jacobian to transform a tip 
position error to a joint error. The joint error is then used to give a 
new reference input to the joint controller. 

A large two-link flexible manipulator at the Flexible 
Automation Laboratory at Georgia Tech was used to implement the 
positioning algorithm. The Landmark Tracking System was mounted 
stationary and observed the manipulator work space. 




Visual Servo 
Control Structures 

Closed-Loop Joint Control 
(Look-and-Move) 

Open-Loop Joint Control 
(Visual Tracking) 
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Visual Feedback 
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Dynamic 
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Position-Based Visual 
Servo (PBVS) 

Image-Based 
Visual Feedback 

Image-Based Look-and- 
Move (IBLM) 

Image-Based Visual 
Servo (IBVS) 




Figure 1.1. Classification of visual servo control 
structures, according to Sanderson and Weiss 

(1983) 
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Figure 1.2. Look-and-move control structure for static 
end-point positioning 









CHAPTER II 


CONCEPTUAL DESCRIPTION 


2.1. Existing Robot Structure and Control System 

The investigated robot manipulator is shown in Fig. 2.1. This 
robot is also known as HALF (Robot Arm Urge and Flexible). The 
manipulator structure consists of two 10 foot long links and a 
parallel link mechanism. Each link is connected to another by a pin. 
RALF is actuated with two hydraulic cylinders, one of them is 
directly connected to the lower link, the other one actuates the 
upper link through the parallel link mechanism. The motion is 
restricted to the vertical plane. More structural details can be found 
in Wilson (1985). 

The robot is equipped with sensors for data acquisition and 
control. Position sensors are mounted parallel to both hydraulic 
cylinders to measure the extension of the rod. Strain gages are 
mounted at several locations on the links. A more detailed sensor 

description can be found in Huggins (1988). 

A DEC Micro-Vax was originally used as a centralized data 

acquisition and control unit. The computer was interfaced with the 
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robot through a Data Translation analog-to-digital converter board. 
A decentralized joint controller was implemented with software 
written in Fortran. 

This implementation had two limitations: Low sampling 
frequency and sequential operation. The maximum sampling 
frequency of the controller was 120 Hz. This frequency was limited 
by a slow sampling rate of the converter board and system overhead. 
An attempt to increase the sampling rate by converting the control 
software to C was not successful. The different programming 
language did not produce "faster" code. 

The sequential control algorithm restricts the operation to one 
predefined motion. After completion of this motion the robot moves 
back to the rest position. With parallel processing it would be 
possible to perform further motion planning, measurements etc 
while the robot is still computer controlled.. 

For those reasons the new control system was implemented on 
a faster computer with fast data acquisition hardware and designed 
for quasi-parallel processing, using multi level interrupts. 


2.2. End-Point Position Measurements and Vision 
System Characteristics 

The vision system used in this investigation is a prototype of 
the Landmark Tracking System (LTS) by Dickerson, et al (1990). A 
landmark is a piece of retroreflective material that is easy to 


detect by the vision system. When the vision system detects a 
landmark in an image, it computes the pixel positions of the 

landmark center with respect to the image. 

For end-point position measurements the vision system was 
mounted facing the manipulator plane of motion and a landmark was 
attached to the manipulator tip. Therefore the vision system 
computes the tip position of the robot with respect to the area 
observed by the camera, assuming the tip-position is identical to 
the position of the landmark. A sketch of the position measurement 
setup is shown in Fig. 2.2. 

The LTS is an independent system that communicates with 
other devices through a serial line. The operation of the vision 
system is controlled with a command language that calls the 
integrated software. A set of thresholds and control flags 
determines operation parameters and output format. 

A position measurement sequence consists of sending a 
command word from the host computer to the LTS and receiving an 
ASCII character string at the serial port of the host computer after 
a delay of approximately 50 ms. The character string contains the 
landmark position information. 

The long delay of the position measurement is a second reason 
to use parallel processing for the new control system. It would not 
be acceptable to wait for the position measurement data to return 
while other tasks are pending. 



2.3. Integrated Control System 


The new control system was implemented on a PC equipped 
with a data acquisition board and designed to process several tasks 
in parallel. 

Quasi-parallel processing can be implemented on a single 
processor computer using interrupt functions. An interrupt function 
is a function that is called by the processor in response to a special 
event. Such a function can temporarily interrupt the normal 
operation. Interrupt functions can therefore be used to perform 
tasks that seem to operate in parallel with other activities. 

Two operation modes will be distinguished per Auslander and 
Tham (1989) for the parallel processing system: Background and 
foreground operations. A background operation is performed in 
response to a user input and has a low processing priority. A 
foreground operation is performed in response to a special event and 
has a high processing priority. 

Two special, time critical functions exist in the control 
system: Manipulator control and serial communication interface. The 
manipulator control consists of an inner joint control loop and an 
outer end-point positioning loop. The time critical inner joint 
control loop was implemented in the foreground as an timer 
interrupt function. The end-point positioning loop was implemented 
in the background as a motion scheduling routine. The serial 
communication interface consists of a receive and transmit portion. 


The receive portion is time critical since a received character could 
be lost if it is not stored from the serial port buffer to a data buffer 
before the next character arrives at the serial port. The receive 
portion of the serial interface is therefore implemented in the 
foreground as a communication interrupt function. The transmit 

portion is implemented in the background. 

All other tasks were implemented as background functions. 

Those functions are a supervisor, a setup option, a measurement 
option and two motion scheduling options. The supervisor of the 
current implementation displays a menu from which to chose the 
options. The setup option is used to change control and vision 
parameters. The measurement option performs single or repeated 
end-point position measurements. Two motion scheduling options 
are available: The first option (move robot) moves the robot to a 
position specified in joint angles, utilizing only joint control. The 
second option (visual servo) moves the robot to an user specified 
end-point position, utilizing joint control and end-point position 
feedback. These background functions communicate with the 
foreground functions through shared data. Fig. 2.3. shows the data 
flow of the control system in block diagram form. 

The joint controller utilizes desired joint angle reference 
inputs and joint angle feedback to compute the control action. Each 
control cycle of the joint controller is started by the system timer. 
Foreground controller and background motion scheduling routines 
communicate through a trajectory data structure. This structure 
consists of a trajectory data array and control flags. 
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Desired joint angles are computed by a background motion 
planning function and stored to the trajectory data array. A 
trajectory control flag is cleared when the complete trajectory is 
stored. Clearing the flag signals the joint controller that new 
trajectory data has arrived and the controller utilizes this data to 
move the robot through the new desired trajectory. The trajectory 
control flag is set again when the controller has utilized all 
trajectory data. Setting of the flag signals the background motion 
scheduling options that a new motion can be scheduled. The joint 
controller holds the manipulator at the last desired joint position 
until a new trajectory arrives in the data array. 


2.4. Algorithm for Vision Supported Positioning 

The manipulator Jacobian relates a small change in joint 
position (joint space) to a change in end-point position (Cartesian 
space) 

Ax = J(0) A©, (2- 1 -) 

where Ax denotes the vector of small end-point position changes, 
J(0) denotes the manipulator Jacobian and A© denotes the vector of 

small joint position changes. 

The inverted manipulator Jacobian can therefore be used to 
transform a tip position error to a joint position error 


A© = J(©)' 1 Ax. 


( 2 . 2 .) 



This process can be applied to an positioning algorithm, consisting 

of the following steps: 

1. measure the end-point position 

2. compute the tip position error 

3. transform the tip position error to joint error, using eq. 2.2. 

4. compute new desired joint position 

5. command joint controller to move robot to the new desired 
position 

Due to system uncertainty the algorithm is iterated until a desired 
end-point positioning precision is reached. Between each correction 
the robot comes to a complete stop to measure the end-point 

position and plan the next motion. 

The conversion of the iterating process is investigated in the 
following. Starting position, desired position, initial tip position 
error and tip position error after the first iteration are sketched in 
Fig. 2.4. The initial tip position error, Axo, is transformed to the 
initial joint error 

A© 0 = ( J*)* 1 Ax 0 , 

where J* is an estimation of the exact Jacobian J. This joint error is 
used to command the robot to the new position, ©o + A© 0 . The 
resulting tip position error after the first iteration is 

Axi = Axo - J A©o- ( 2 - 4 .) 


Substituting eq 2.3. in eq. 2.4. yields 
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AX! « (I - J(J’)- 1 ) Ax 0 . (2.5.) 

Assuming the change of the exact Jacobian J is small and the 
estimate J* is constant, the resulting tip position error after n 
iterations becomes 

Ax n = (I - JfJ*)- 1 )" Ax 0 . (2.6.) 

By modal transformation of Eq. 2.6. we yield 

Ax n ' * A n Axo’, (2.7.) 

where A is the diagonal matrix of eigenvalues of (I - J(J*) _1 ). The 
algorithm converges for n -> «> and the tip position error goes to 
zero when the largest eigenvalue of (I - J(J*) _1 ) satisfies the 
condition 


IWI<1- (2-8.) 

If J* is a good estimate of the true Jacobian J, the product J(J*) _1 is 
approximately the identity matrix and (I - J(J')- 1 ) is approximately 
the null matrix. In this case the condition eq. 2.8. is satisfied. The 
better the estimate J\ the faster the algorithm will converge. 

When the value of the exact Jacobian undergoes only a small 
variation in the work space under consideration it is sufficient to 
use a constant estimation of the Jacobian. This estimate is 
determined using a calibration described in chapter 4.5. 
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Figure 2.1. RALF (Robot Arm Large and Flexible) 
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resulting workspace 
for position measurements 



Figure 2.2. Setup for end-point position measurement 


* 
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joint controller 


serial communication 
interface 



robot position 


Figure 2.3. Control system block diagram 
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Figure 2.4. End-point positioning, position error after 
first iteration 
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CHAPTER III 


VISION SYSTEM 


3.1. Principle of Landmark-Based Position Measurements 


The high cost and complexity of existing vision systems 
motivated researchers at Georgia Tech to develop a low cost vision 
system for use in locating objects in a manufacturing environment. 
This vision system uses landmarks that are attached to these 
objects to yield precise position measurements quickly. 

A landmark is easy to distinguish from the environment and 
background noise because of it’s brightness and known shape. A 
landmark can be a light source (LED, light bulb) or a piece of 
reflecting material. The light source has the disadvantage of less 
flexibility, especially when several landmarks are used. The 
reflecting material requires a light source for illuminating the 
landmark, but this can also be an advantage since a short 
illumination period can be used to "freeze" an image. 

Three modes of reflection can be distinguished: Specular 
reflection (plane mirror), diffuse reflection and retroreflection. 
Retroreflection is especially useful for landmark tracking because it 
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does not occur naturally and the reflected light returns along the 
line of the incident light. The intensity of light returned from a 
retroreflective landmark is very high, resulting in a good contrast. 
The LTS is typically used with retroreflective landmarks. 

The properties of retroreflection have to be considered in 
illumination design. The intensity of the reflected light decreases 
rapidly with increasing observation angle. This angle is explained in 
Fig. 3.1. Therefore the illumination source has to be very close to the 
optical axis. The intensity of the reflected light is much less 
sensitive to a variation of the entrance angle, permitting a large 
field of view. 


3.2. Components of the Landmark Tracking System 


3.2.1. Optics and Illumination 

Some disadvantages of lenses can be avoided by using a pinhole 
optics for landmark tracking, e.g. lense optics require focusing and 
yield an image with barrel or pincushion-like distortions. Pinhole 
optics give a large depth of field and do not have the geometric 
distortion of lenses. The typical disadvantages of a pinhole optics, 
an unsharp image and a high f/stop, are less important or even 
advantageous for landmark tracking. A sharp image is not necessary 
to determine the landmark position and a high f/stop is useful to 
suppress other objects. 
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Pinhole optics requires a powerful illumination, especially for 
long range measurements. A very short and intense illumination is 
desirable for dynamic position measurements to "freeze" the motion 
and yield a high signal-to-noise ratio. Xenon strobes with a period of 
light of approximately one millisecond show these properties and 
are therefore typically used with the LTS, like in the current 
approach. The strobe is mounted very close to the optical axis to 

keep the observation angle small. 

An electronic flash unit, consisting of a charge circuit, a 
trigger circuit and timing logic is used to operate the strobe. The 
flash unit is based on designs by E.H. Lee and Dickerson (1990) and 
K.-M. Lee, et al (1991). A pulse signal from the LTS processor times 
the charge time of the capacitors and the triggering of the flash. The 
energy stored in the capacitors is discharged in the flash tube after 
a flash is triggered. The intensity of a flash is proportional to the 
discharged energy. 

Two illumination designs were tested, both designs are shown 
schematically in Fig. 3.2. Design A, the original design for the LTS, 
was tested with a small flash tube with a maximum input power 
rating of 1.5 Watt. The maximum input power limits the flash rate 
since an energy of approximately 0.5 Joule is necessary for 
sufficient illumination of the landmark at the robot tip. Therefore 
design B was tested, using an U-shaped flash tube with a maximum 

input power rating of ten Watts. 

Unfortunately design B did not produce flashes with the same 
intensity as design A. The manufacturer of the flash tubes explained 
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this resulted from operation of the small flash tube at its maximum 
trigger voltage, claiming this flash tube would operate beyond it's 
limits. An attempt to get a brighter flash with design B by 
increasing the trigger voltage did not succeed. This issue was not 
investigated further, instead design A was used, limiting the rate of 
position measurements severely. 

Note: A tube similar to the one used in design A is also 
available with three Watt maximum input power and two tubes could 
be mounted in parallel on both sides of the sensor. Design B should 
be reconsidered for a further increase of the flash rate since there 
is also a different trigger transformer available. 


3.2.2. LTS Hardware 

The LTS consists of three functional units: Video head, strobe 
unit and computer. The video head combines pinhole optics, a CCD 
array sensor and an 8-bit analog-to-digital converter. The strobe 
unit consists of a flash tube and an electronically controlled power 
unit as described in the previous paragraph. The computer is a 68000 
microprocessor with a 68901 multi-function peripheral device and 
memory. All operations of the LTS are controlled by the 68000, 
while the 68901 is utilized as a timing and communication 
interface. A block diagram of the LTS is shown in Fig. 3.3. 

An image acquisition process is started by sending a pulse to 
the strobe unit to charge the capacitors and trigger the flash. During 


it 
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the period of strobe discharge the internal software prevents any 
charge shifting on the CCD sensor. After the illumination time the 
charges are shifted out of the sensor, amplified, and converted to 
pixel intensities. The pixel intensities are stored to the video RAM 
which is disconnected from the 68000 during the shifting process. 
After the image acquisition process is completed, the 68000 
accesses the data in the video RAM for further processing, depending 
on the software. The LTS contains 64 kB EPROM primarily intended 
for software and 64 kB additional RAM for data processing. 

The 68901 provides an asynchronous receiver-transmitter that 
is utilized to communicate with an external host computer through a 
RS-232 serial line. The original LTS was limited to a Baud rate of 
2400. Therefore an additional counter was added to the circuit board 
for the selection of Baud rates up to 57.6 kBaud. The system clock 
was changed for this modification from 16 MHz to 18.432 MHz. In 
this special case changing the system clock did not require an 
additional clock for the communication rate and also increased the 
processing speed by 15%. 


3.2.3. LTS Software 

The host computer accesses LTS software through messages 
sent to the LTS. Messages are in ASCII and one or more bytes long. 
ASCII characters were originally chosen to operate the LTS from a 
terminal; this was not changed to binary since the control system 
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offers a similar terminal option. The first byte determines the 
intent of the message (command), the remaining bytes are 
parameters. The prompt from the LTS is telling the host 
computer that the LTS is ready to receive a message. An error in a 
command returns a second prompt, a parameter error returns a 
backspace. All characters received from the host are returned 
immediately, the response from the LTS follows. 

Some of the available commands are explained in the 
following: 

picture (P), this command stores a complete picture from the 
CCD array to video memory. The routine handles all necessary 
timing, including the charging and firing of the strobe. 

locate (Ln), locates landmark(s) in window n. The landmark 
area, peak value, row (Y) C.G. and column (X) C.G. are returned as an 
ASCII character string, the output can be modified using the 
threshold command. This algorithm scans the specified window for a 
bright pixel (threshold 0), investigating only every other row and 
every other pixel. When a bright pixel is found the landmark is 
systematically expanded by comparing the intensity of adjacent 
pixels to threshold 1. 

picture & locate (Kn), takes a picture and locates 
landmark(s) in window n. This command combines the picture and 
locate commands. 

show (Sn), creates a crude image display on screen, using 
window n. The image in the video buffer is displayed on 24 rows and 
78 columns, the intensity is displayed as a hexadecimal number. 
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window (wnlrlchrhc), this command defines window n. The 
four bytes low row (lr) f low column (Ic), high row (hr) and high 

column (he) define the boundaries of window n. 

threshold (tnxx), the byte xx is the new value of threshold n, 

the available thresholds are explained in Table 3.1. 

flag (fnx), sets the flag n to the value x, where x=0 is false 

and x=1-F is true. Available flags are explained in Table 3.2. 

3.3. Performance of the Landmark Tracking System 

The performance will be evaluated in this chapter, using as 
parameters sampling time, resolution and range. These three 
parameters are equivalent to speed, accuracy and area of the 
measurement and are not independent. Resolution and range as well 
as sampling rate and range are inversely proportional. The chosen 
configuration has to balance those three performance measures. 

Resolution and range are more important for the static 
positioning system than sampling time. Range is particularly 
important since the investigated manipulator has a very large work 
space. For these reasons it was decided to chose a range of six feet 
between camera and manipulator, which results in an observed area 

of 43 by 43 inches. 
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3.3.1. Sampling Time 

This section investigates the parameters determining the 
sampling time of the vision-based position measurement and 
describes modifications that improved the sampling rate. Some 
future improvements are suggested. 

The sampling time is composed of six segments due to the 
sequential operation of the LTS: 1. the communication time to send a 
command to the LTS. 2. the charge time of the strobe unit. 3. the 
time for shifting the vision data out of the CCD array and storing 
them to memory. 4. the computation time of the landmark tracking 
algorithm. 5. the communication time to send the landmark data to 
the host computer. 6. the time to process the raw character string in 
the control system. Only the first five periods are influenced by the 
LTS, the last time is not further investigated. 

Values for the first five periods and the resulting sampling 
time of the LTS are given in Table 3.3. for three variations of the 
LTS: the original system, the currently used system and a fictitious 
future system. The four components (the two communications are 
combined) of the sampling time are discussed separately in the 
following sections: 

First, the charge time of the strobe unit was measured with an 
oscilloscope. The charge time depends on the flash energy necessary 
for a sufficient illumination of the landmark, therefore on the range 
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of the position measurement, and is relatively long for the 
investigated case. The charge time was decreased in the current 
system by hardware changes in the charge circuit, and can be 
software controlled through the setting of threshold 4. The relation 
between threshold setting, charge time and flash energy for the 
currently used strobe unit is displayed in Table 3.4. For a further 
decrease of the charge time it is suggested to operate the strobe 

unit in parallel with the other LTS functions. 

Second, the time for the data shifting can be calculated from 
the number of pixels (165 row pixels x 192 column pixels) and the 
shifting rate. This time was decreased by using a faster system 
clock since the shifting rate is proportional to the system clock. 
This time is independent from other parameters discussed in this 

paragraph. 

Third, the time required for the processing was estimated 
from the difference between total time and the remaining 
components. The processing time depends on the size of the 
landmarks used (in terms of pixels in the image) and on the area 
scanned for landmarks (window). The time in Table 3.3. was 
determined for scanning the whole image and tracking two 
landmarks. The processing time was decreased by using a faster 
clock and reducing computations, since second moments of the 
landmarks are not calculated. A future development should decrease 
the processing time by using a windowing algorithm that predicts 
the next landmark location and scans only a fraction of the image. 
Nam and Dickerson (1991) for example have decreased the 
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processing time to approximately 8 ms by scanning 1/50 of the 
image and using one landmark with a size of approximately 50 
pixels. 

Fourth, the communication time can be calculated from the 
number of transmitted bytes and the Baud rate. This time was 
identified as the major bottleneck and decreased by using a higher 
Baud rate and transmitting fewer bytes. A small hardware change, 
described in chapter 3.2.2. enabled Baud rates up to 57600 Baud. The 
experiments described in this thesis were performed using 19200 
Baud. This has two reasons: First, at 57600 Baud a character is 
received at the host every 0.17 ms and this would slow down the 
current controller as explained in chapter 4.1 . Second, 57600 Baud 
worked well with the laboratory setup but the RS-232 standard only 
supports Baud rates up to 20000, Lee, K.-M., et. al. (1991) The 
number of bytes transmitted from LTS to host computer depends on 
the number of data bytes and the number of echoed characters. 
Sending the data as digits or ASCII characters also influences the 
number of transmitted bytes. Table 3.5. lists the transmitted bytes. 
The original system transmitted 45 bytes (40 ASCII, data bytes for 
two landmarks and 5 echoed characters , P?Ln?') at 2400 Baud. The 
current system transmits 27 bytes (24 ASCII, data bytes and 3 
echoed characters 'Kn?'). 

Area and Peak are not necessary for the position measurement 
but are still transmitted for error checking. Sending those bytes 
could be eliminated if the error checking would be performed by the 
onboard software. Data is still transmitted in ASCII to support a 
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terminal option of the control system; modifying this option in the 
future could further reduce the number of transmitted bytes. 

The minimum sampling time of the used system was measured 
to be 95 ms. However, this sampling time is not used for normal 
operations since the strobe frequency is currently limited by the 
available flash tubes. 

3.3.2. Resolution of the Position Measurement 

This section describes a repeatability test and discusses the 
resulting resolution of the position measurement. The range of the 
measurement ultimately determines the resolution. 

The repeatability of the position measurement was first 
tested with an initial setup on a workbench. The most important 
result of those initial tests was that the prototype LTS should only 
be used for relative position measurement. The software controlled 
timing of the data shifting caused a random shift in the column 
value of the center of gravity of the landmark. This error is 
eliminated when the distance between two landmarks is measured, 
because the C.G.'s of both landmarks are shifted by the same amount. 
Newer versions of the LTS have a hardware controlled timing of the 
data shifting that eliminates the shifting error. 

The repeatability of the relative position measurement was 
tested under setup conditions. The vision system was mounted on the 
camera frame, viewing the work space plane of the manipulator. Two 
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landmarks, 21" apart, were attached to a level and the level was 
placed in the manipulator plane. The repeatability was tested for x- 
and y -direction separately. The test results are summarized in Table 

3.6. 

The repeatability is better in x-direction than in y-direction, 
where the ratio of the repeatabilities is approximately equal the 
ratio of pixels in these directions (192 x-pixel, 165 y-pixel). The 
repeatability expressed in length instead of pixels is directly 
proportional to the range of the measurement. 

Estimating that the accuracy is approximately 1/10 of the 
repeatability, a position measurement accuracy of 1.5 - 2.0 mm 
should be expected. This approximation seems reasonable since the 
worst case results of the repeatability test are approximately twice 
the size of the variation. 


A 


Table 3.1. Parameters used by the Landmark Tracking 
System (threshold command tn) 
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LTS 

control 

threshold description 

default 

(hex) 

system 

(hex) 

minimum value for pixel to start 
growing a landmark 

30 

30 

minimum value for pixel to be included 
in landmark 

20 

20 

minimum area of blob to be reported 
as landmark 

04 

1 0 

integration time for exposing CCD 
= 0.32 (t3 + 0.4) ms, with 16 MHZ clock 

03 

03 

strobe charge time = 0.28 t4 ms, 
with currently used strobe 

oo 

40 

repeats of integration time 
(threshold 3) 

00 

00 

configuration of landmark output, 
returned when bit cleared: 
bit 0 - landmark area 
bit 1 - peak value 
bit 2 - row (Y) C.G. 
bit 3 - column (X) C.G. 
bit 4-7 are not used currently 

00 

(area, 

peak, 

C.G's 

returned) 

00 

(area, 

peak, 

C.G's 

returned) 
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Table 3.2. Flags used by the Landmark Tracking System 


n 


0 


flag description default 


all received characters are returned (echoed back) true 
to the host computer 


1 


all 

just 


landmarks in a window 
the first landmark 


will be recognized, not 


true 
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Table 3.3. Sampling time for three variations of the 
Landmark Tracking System 


LTS 

sampling 

time 

(ms) 

original system 
scanning whole 
image, 2 landmarks 

current system 
scanning whole 
image, 2 landmarks 
(22 pixels) 

future design 

communication 

12.5 

1.0 

0.5 

time, host to LTS 

3 byte at 2400 Baud 

2 byte at 19200 

1 byte at 19200 

strobe charge 

41 

18 

- 

time, 0.5 J, 6' 

sequential operation 

sequential operation 

parallel operation 

data shifting 

7.9 

6.9 

6.9 


4 MHz 

4.6 MHz 

4.6 MHz 

processing time 
& overhead 

? 

55 

4.6 MHz, estimated 
from total time 

? 

communication 

187.2 

14.0 

2.1 

time, LTS to host 

40 data bytes, ASCII 

24 data bytes, ASCII 

2 data bytes, hex and 


and 5 echoed bytes 

and 3 echoed bytes 

2 echoed bytes at 


at 2400 Baud 

at 19200 Baud 

19200 Baud 

total time 

> 311.9 

95 

< 64.6 


estimated 


timed with PC 


estimated 






















39 


Table 3.4. Relation between threshold setting, charge 
time and flash energy for currently used 
strobe unit 


threshold t4 


charge time 


voltage 


diff. 


flash energy 


(hex) 


At (ms) 


AV (V) 


E « 0.5*C*AV 2 


08 

10 

18 

20 

28 

30 

40 

50 


2.22 

4.55 

6.66 

8.9 

11.05 

13.25 

17.80 
22.15 

26.80 




4.6 

.00033 

48.4 

.036 

76.4 

.091 

110 

.19 

129 

.26 

148 

.34 

178 

.49 


60 


199 

207 


.61 

.66 
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Table 3.5. Bytes transmitted from Landmark Tracking 
System to host computer 


transmitted bytes send as digit send as ASCII char 

area of landmark 1 2 


peak intensity 
row center of gravity 
column center of gravity 
row 2nd moment 
column 2nd moment 


2 

4 

4 

4 

4 


echoed characters 


1 per character 




Table 3.6. Repeatability of the relative position 
measurement 


Repeatability 

(pixel) 

(inch) 

(mm) 

X-direction (col): 




variation 

0.0287 

0.00654 

0.166 

worst case (+) 

+0.0667 

+0.0152 

+0.386 

worst case (•) 

*0.0633 

-0.0144 

-0.366 

Y-direction (row): 




variation 

0.0309 

0.00807 

0.205 

worst case (+) 

+0.07 

+0.0183 

+0.464 

worst case (*) 

-0.05 

-0.0131 

-0.332 


conditions: 120 measurements for each direction, 6’ range, 

2 landmarks, 21" apart 
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retroreflective 

landmark 



Figure 3.1. Optical elements in the Landmark Tracking 
System, according to Lee and Dickerson (1990) 
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Design A - straight flash tube 



optical axis 



CCD - sensor 


Design B - U-shaped flash tube 


I 



optical axis 



Figure 3.2. Illumination design using Xenon-strobes 



44 



to external 
host computer 

Figure 3.3. Components of the Landmark Tracking System, 
according to Dickerson et al (1990) 






a 
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CHAPTER IV 


CONTROLLER IMPLEMENTATION 


The control system was implemented on a PC equipped with a 
data acquisition board. Time crucial operations of the system were 
implemented as interrupt service routines while all other operations 
were implemented in the background. The software was organized in 
modules and mostly written in C, some assembler code is used for 
interrupt functions. 


4.1. Programming with Interrupts 

Two operations of the control system were implemented as 
interrupt service routines; Joint control and the receive portion of 
the serial communication. 

The joint control was implemented as a timer interrupt 
service routine to guarantee a constant sampling time. The signal 
for a timer interrupt is generated by a 8253 programmable interval 
timer that is on the PC system board and normally used for the 


46 


time-of-day interrupt. The interval of the timer is set to the 
sampling time of the joint controller by writing to the timer control 
port, (file alarm.c, taken from Auslander and Tham, 1989). The 
minimum sampling time of the joint controller is approximately 1.6 
ms but the sampling time was set to 5.0 ms to give the computer 
enough time to process the other tasks. 

The receive portion of the serial communication was 
implemented as a communication interrupt function (coml). The 
signal for a communication interrupt is generated by a 8250 UART 
(universal asynchronous receiver/transmitter) that implements the 
serial communication interface of the PC. This signal is generated 
every time a byte is received and stored in the UART data buffer. The 
communication interrupt service routine stores this byte to memory 
before the next byte is received since the UART data buffer can 
store only one byte. No handshaking is implemented for the 
communication between PC and LTS while at a communication rate 
of 19200 Baud a byte is received approximately every 0.5 ms. 
Therefore the communication interrupt needs a higher priority than 
the timer interrupt since bytes could be lost otherwise. The 
processing time of the communication interrupt service routine is 
short compared to the processing time of the timer interrupt service 
routine. Therefore the controller sampling time is not altered 
significantly when the communication interrupt temporarily 
suspends the processing of the timer interrupt. 

Both interrupt signals are send to a 8259A programmable 
interrupt controller that handles hardware interrupt requests to the 
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processor of the PC. The interrupt controller can be programmed to 
assign priorities to external interrupt requests. The standard 
configuration assigns the highest priority to the timer interrupt and 
a lower priority to the coml interrupt. However, assigning a higher 
priority to the coml interrupt was not accomplished by just 
reprogramming the interrupt controller. Instead the timer interrupt 
service routine was programmed so that it can be interrupted by the 
communication interrupt. This is briefly explained in the next 
section, for a more thorough explanation of the interrupt processing 
sequence the reader is referred to the technical PC literature, e.g. 
Tomkins and Webster (1987), Eggebrecht (1990). 

An interrupt request of the interrupt controller is only 
acknowledged by the central processing unit (CPU) when the 
interrupt flag is set (unmasked). During the interrupt sequence the 
interrupt flag is cleared (masked), so that no other interrupt 
requests are acknowledged while the current request is processed. 
The return-from-interrupt (IRET) command unmasks interrupts again 
after the interrupt service routine is completed. This can be altered 
by setting the interrupt flag during the processing of the interrupt 
service routine with the set-interrupt-flag (STI) command. This 
enables other interrupts to interrupt the currently processed 
interrupt service routine. In this case a flag should be used that 
checks if the routine is not self-interrupting. 
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A second issue is important for using the interrupt controller. 
The interrupt controller does not process further interrupt requests 
until it receives an end-of-interupt (EOI). This end-of-interrupt 
must be sent by the hardware interrupt service function. 

Those two issues are considered in the following way: The 
communication interrupt service routine can't be interrupted. 
Therefore a specific end-of-interrupt (SEOI) is sent to the interrupt 
controller immediately before returning from the communication 
interrupt service routine. The specific end-of-interrupt resets the 
request for a com 1 interrupt. The timer interrupt service routine 
can be interrupted by the communication interrupt. Therefore a SEOI 
is send to the interrupt controller and a STI is send to the CPU 
before the actual control routine is called by the timer interrupt 
service routine. In this case the SEOI resets the request for a timer 
interrupt. 

Some other issues should be mentioned that were considered 
for the interrupt implementation: 

1. The math-coprocessor state is saved at the beginning of the 
timer interrupt service routine since joint controller and background 
routines use floating point calculations. This state is restored at 
the end of the service routine. 

2. Non-reentrant functions can't be called by the foreground 
and background routines at the same time. Math functions, BIOS 
functions and functions interfacing the data acquisition board are 
non-reentrant functions. Those functions have in common that they 
address some special hardware components. Therefore the state of 
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those functions is determined by the processor state and the state 
of the hardware registers. Since the hardware registers are not 
saved and restored by an interrupt function, the processor could not 
continue a interrupted function correctly when a previous interrupt 
altered those registers. Therefore non-reentrant functions are not 
shared by foreground and background routines. The data acquisition 
board is only accessed from the foreground routines. The sensor 
readings are then stored in external memory for access by the 
background. Print statements that are supported by BIOS functions 
are only used in the background. 

3. A program section that can't be interrupted is called a 
critical region. For example a critical region exists when a 
background routine writes shared data that a foreground routine 
should not access until the writing is completed. Such critical 
regions were implemented using control flags. Other critical regions 
exist at the beginning and end of the timer interrupt service routine. 
These regions include saving and restoring the coprocessor state, 
checking for self-interrupting and sending the SEOI to the interrupt 
controller. The interrupt flag of the CPU was masked in these 
critical regions, disabling further interrupts inside the regions. 

4. No automatic variables are used in the foreground since this 
causes stack-overflows. Automatic variables are created on the 
stack after entering a function and exist only for the duration of the 
creating function call. The foreground accesses only static or 
external variables which are assigned to a particular memory 
location and therefore exist for the duration of the whole program 
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execution. Unfortunately static memory is limited to 64K, therefore 
the size of the arrays for data acquisition and trajectory data was 
limited. This is alleviated using a ring buffer for data acquisition 
and linear interpolation between trajectory points. 

5. The DOS operating system of the PC normally uses the time- 
of-day interrupt to update the system clock and maintain other 
system functions. Therefore the "old" timer interrupt service routine 
is called by the "new" one every 55 ms to maintain those system 
functions. 


4.2. Joint Controller Module 

The joint controller module (file control2.c) consists of the 
timer interrupt service routine, control routine, trajectory data 
retrieving routine and data storing routine. 

The timer interrupt service routine (tmr_isr) has several 
tasks: 1. it handles the correct interrupt processing described in the 
previous chapter by saving and restoring the math-coprocessor 
state, calling the DOS timer interrupts service routine every 55 ms, 
sending the SEOI to the interrupt controller and checking a flag to 
prohibit self interrupting. 2. it calls the control routine. 3. it keeps 
track of time by counting the calls to the timer interrupt service 
routine and 4. it calls the data storing routine. 

The control routine (control) implements a PD controller that 
is based on Yuan's control algorithm (1989). Huggins's (1988) gain 
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scheduling algorithm is used to compute configuration dependent PD 
gains. Two proportional gains were added to the algorithm for 
controller tuning; the joint position error is multiplied by these 
gains. These gains are stored in an external gain structure (gainl) 
and can be modified from the background during operation. These 
gains were set to 1.1 and -1.4 for joint one and two respectively; for 
tuning the gains were varied from 1.1 to 1.4 and -1.1 to -1.6, but the 
steady state joint position error was not sensitive to these 
variations while the higher values caused more tip vibrations. 
Averaging multiple joint sensor readings reduced the steady state 
error by a factor of two to the value of 0.005 radians for both joints. 

The current joint angles are retrieved from the joint position 
sensors by accessing the analog-to-digital converter and converting 
the digital values to angles. The joint angles are stored as external 
variables (thl actual, th2actual) that can be accessed by the 
background. Routines to access the analog-to-digital converter and 
for unit conversions are collected in separate files (adstuff.c and 
convers.c). The control outputs are digital values; these values are 
converted to voltages by the digital-to-analog converter and sent to 
the amplifiers that control the hydraulic valves. 

The desired joint angles are retrieved by the trajectory data 
routine (get_traj). This routine reads the external trajectory data 
structure (trajl) and determines the desired joint angles depending 
on the current control status. The control status is indicated by the 
trajectory control flag: The flag is set when the controller holds the 
robot at the last desired position and the flag is cleared when the 
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controller follows a trajectory. In the first case the desired joint 
angles are stored as external variables (th1d_hold, th2d_hold). In 
the second case the desired joint angles are determined from the 
trajectory data array. 

Depending on the duration of a trajectory the desired joint 
angles are directly taken from the data array or are computed by 
linear interpolation between trajectory points. This results from 
memory constraints, since for a long trajectory a desired pair of 
joint angles can not be stored for every control cycle. The number of 
interpolations is stored in a variable of the trajectory data 

structure (iterations). The trajectory data routine computes the 
desired joint angles by keeping track of the number of processed 
trajectory points and interpolations. 

The data storing routine (store_data) stores time, actual and 
desired joint angles and control action to a ring buffer. The ring 
buffer is implemented as an external data structure (datal). A 
"fuir-flag and a status-flag are used to control the buffer. The 

status flag can be set by the user and determines if data is stored or 

not. The "full"-flag indicates if space is available, no data is stored 
when the "fuir-flag is set. This flag is set by the data storing 
routine when the buffer is full. A background routine writes the 

content of the ring buffer to a data file and clears the "full"-flag. 


4.3. Motion Planning Module 


The motion planning module (file move_rob.c) implements the 
trajectory planning algorithm and two motion control options. 

Trajectory planning consists of two routines (plan_traj and 
move_robot_auto), the first routine implements the actual 
trajectory planner, the second routine determines parameters for 
the trajectory planning. 

The trajectory planning algorithm is based on the previous 
implementation. Fourth order polynomials, divided into eight 
sections yield a smooth trajectory. The trajectory is planned for 
straight line tip motion and converted to joint angles using rigid 
kinematics. A work space boundary test algorithm keeps the entire 
trajectory within the manipulator work space. Initial and desired tip 
position, maximum tip velocity and duration of motion are 
parameters of the algorithm. The tip position, which is specified 
relative to the manipulator base, is an estimate of the exact tip 
position since it is related to joint angles by rigid manipulator 

kinematics. 

The step size of the trajectory, i.e. the number of control 
cycles between trajectory points, depends on the duration of motion. 
For memory constraints, the static memory is limited to 64K, the 
trajectory is limited to 200 points. Linear interpolation between 
trajectory points is used if more than 200 control cycles are needed. 
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At a controller sampling rate of 200 Hz, this becomes the case when 
the duration is more than one second. The trajectory planning 
algorithm computes the necessary number of iterations and writes 
it to the trajectory data structure, while the linear interpolation is 
done during the control cycle by the trajectory data retrieving 
routine. 

The second trajectory planning routine determines the duration 
of a motion when this time is not directly specified by a user input. 
A relation for the time of a motion was experimentally determined 
and is given in Table 4.1., this relation depends on the travelled 
distance. 

The first motion control option is a pure joint control, the 
robot is moved to a position specified in joint coordinates. This 
option is implemented in the routine move_robot, consisting of user 
interface, motion planning and data saving. The user interface 
prompts the user for desired robot position and duration. The desired 
position can be specified in joint angles or as tip position. With this 
data the trajectory planning algorithm is called. Data stored in the 
ring buffer is written to a data file by the w_data_to_disk routine. 
This routine also clears the "fuH"-flag of the data buffer. The actual 
robot motion is started by clearing the trajectory control flag which 
indicates the joint controller that a new trajectory has arrived. 

The second motion control option implements the static end- 
point positioning algorithm described in chapter 2.4. The routine 
vision_guided_pos moves the robot to an end-point position 
specified by the user. The algorithm first checks if the manipulator 
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tip is already in the area observed by the camera, this is simply done 
by taking a picture and searching for the landmarks. If the tip 
landmark is detected, the algorithm computes the first set of 
desired joint angles based on the tip position error. Otherwise the 
joint angles are estimated from the desired tip position. After this 
initial move end-point position feedback is utilized to update the 
reference input to the joint controller after every correction move. 
This updating continues until the end-point position is within a 
predefined tolerance of 0.06 inch. Both trajectory planning routines 
are utilized to move the robot to the final position without further 

user input. 


4.4. Vision Module 

The vision module (file vstuff.c) combines routines to retrieve 
the tip-position from the Landmark Tracking System. A position 
measurement command (get_position) consists of the following 
tasks: 1. sending the picture & locate command (Kn) to the LTS, 2. 
receiving the landmark data from the serial communication buffer, 
3. checking the landmark data for errors and determining reference 
landmark and landmark at the robot tip, 4. compensating for the 
reference landmark position, 5. computing the position of the 
"moving" landmark relative to the reference landmark. 

The communication interrupt service routine stores characters 
returned from the LTS in the external landmark data array (lm_data). 
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This data array is inspected for the landmarks. The reference 
landmark is detected by comparing the position of the returned 
landmarks to the expected position. The remaining landmark is the 
landmark at the robot tip or when more than one landmarks are 
remaining the landmark is detected by comparing area and peak 
intensity to expected values. 

The reference landmark position is compensated by an 
experimentally determined factor since this landmark is not placed 
in the same plane as the tip landmark. With this compensation the 
computed tip-position is the relative distance between both 
landmarks, projected in the plane of the manipulator tip. 

Conversion factors to convert pixels to length were 
determined experimentally, similar to the repeatability 
measurement described in chapter 3.3.2. A known distance is 
compared to the measured number of pixels to yield the conversion 
factor. This experimental method was used instead of computing the 
conversion factors from field of view and range since the distance 
between LTS and manipulator is not easy to measure. 


4.5. Initialization Module 

The initialization module (file setup.c) combines routines for 
default system initialization, parameter modification, Jacobian 
calibration and system shut down. 
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The default system initialization (def_init) performs the 
following tasks: 1. The ring buffer for data storing is initialized. 2. 
The manipulator Jacobian is retrieved from data file. 3. The data 
acquisition hardware is initialized. 4. The user is prompted for the 
initial communication rate and the serial communication is 
initialized. 5. The user is prompted to reset the LTS, new defaults 
are written to the LTS and a first picture is taken (since the first 
picture contains garbage). The new camera defaults are stored in an 
external data structure (v_config) that is used as an internal 
representation. 6. The controller gains are set to default values. 7. 
The trajectory data structure is initialized. The control flag is set 
for holding and the desired joint angles are set to the current joint 
angles. 8. The 8259 interval timer is set to the controller sampling 
time. 9. The DOS timer interrupt service routine is replaced by the 
new routine for the control system. 

The changes to the DOS defaults are reversed before the 
control system shuts down (clean_up): 1 . The old DOS timer interrupt 
service routine is restored. 2. The 8259 interval timer is set to the 
DOS default of 55 ms. 3. Zeros are written to the output ports of the 
digital-to-analog converter to drive the robot to it’s rest position. 4. 
DOS defaults for serial communication are restored. 

Modifications of the system defaults can be chosen from the 
initialization menu. This menu offers the options parameters, 
terminal program and Jacobian calibration. 

The first option calls the parameter menu that offers the 
following choices: 1. Controller gains. The controller gains are 
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stored in an external data structure and can be modified during 
controller operation. 2. LTS thresholds. Thresholds 0-5 from Table 
3.1. can be modified with this option. The system writes the new 
threshold to the LTS and keeps an internal record (v_config) for 
display purpose. 3. Baud rate and data format for serial 
communication. 

The Jacobian calibration option determines a constant value 
estimation of the manipulator Jacobian for the work space area 
observed by the LTS: 1. data points are collected, by moving the 
manipulator through a series of points and recording tip position and 
joint angles. 2. the Jacobian is determined from the least square 
estimation (Searle, S.R. and Hausman, W.H., 1970): 

J‘T«( A© t A0 J- 1 A©T Ax (4.1 .) 

where J* is the estimate of the Jacobian, A© is the matrix of joint 
angle changes between consecutive data points and Ax is the matrix 
of tip position changes between data points. 3., the Jacobian can be 
stored to a data file for later use. 
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Table 4.1. Duration for trajectory planning 


max joint angle 
difference for 
desired motion (rad) 

duration (seconds) 

0 - 0.04 

0.8 + A© / 0.05 

0.04 - 0.1 

0.8 + A© / 0.08 

>0.1 

0.8 + A© / 0.1 
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CHAPTER V 

VERIFICATION OF CONCEPT 


The static positioning algorithm was tested with two 
experiments. The first experiment investigated the conversion of the 
algorithm and the second experiment investigated the positioning 
repeatability under payload variation. The tip position of the 
manipulator is in the following assumed to be identical with the 
position of the landmark located at the manipulator tip. 

5.1. Convergence of the Algorithm 

To test the convergence of the positioning algorithm the robot 
was started from several locations inside and outside the camera 
work space and commanded to a tip-position specified in camera- 
coordinates. The positioning process was continued until the tip 
position error was less than 0.06” or the number of positioning 
iterations exceeded a limit of 5 iterations. The robot was monitored 
with joint sensors and Landmark Tracking System. 
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The tip-position during a typical positioning sequence shows 
Fig. 5.1. for the case that the robot is started with the tip inside the 
camera work space. The robot is started from the position 
25.9", 22.1" (outside the figure) and reaches the proximity of the 
desired position at U.5", -4.55" after three positioning motions (an 
initial move and two small corrections). Joint angle, joint angle 
error and control action during the positioning are shown in Figures 
5.2. - 5.4. The control actions are digital values; these values are 
converted to voltages by the digital-to-analog converter and sent to 
the amplifiers that control the hydraulic valves. Those plots show 
how the desired joint angles are updated to reach the desired tip 
position in spite of a steady state error of the joint control. 

The tip-position during a positioning sequence started outside 
the camera work space is shown in Fig. 5.5. In this case the 
algorithm needs four corrections after an initial move to an 
estimated position. 

During all experiments the algorithm positioned the robot with 
less than 5 iterations after the initial move. When started inside the 
camera work space the robot was typically positioned after three 
moves. Starting the robot outside the camera work space resulted in 
one or two additional iterations since the estimation of the first 
desired position does not utilize end-point position feedback. The 
remaining tip position error, as measured by the LTS, was typically 


0.03". 


5.2. Positioning Experiment 


A second experiment investigated the positioning repeatability 
under payload variation. The static end-point positioning algorithm 
was compared to pure joint control. The robot was initially 
positioned at a rack in the work space and the position of a pointer 
attached to the tip was manually measured with a scale. Landmark 
position and joint angles at this position were measured for later 
use as desired inputs. Then the robot was repeatedly commanded to 
the initial position, using one of the two controller options. Joint 
angles, landmark position and scale measurement were reported 
after each positioning. The scale measurement was used as a quasi- 
reference; the resolution of this measurement is less accurate than 
the LTS, but it can be used to confirm the qualitative trend of the 
positioning repeatability. The same procedure was repeated with a 
55 lb payload attached to the manipulator tip. 

Measurement results are summarized in Table 5.1. The second 
column displays an estimate of the tip position (converted from 
joint angles by rigid kinematics), the third column displays the 
measured landmark position and the fourth column displays the 
reference measurements from the scale. The fluctuation of the tip 
position due to payload variation was computed from this data and is 
shown in Table 5.2. 
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The payload causes the tip to deflect vertically approximately 
one inch when only the joints are controlled. The end-point 
positioning algorithm is able to compensate for this deflection. 


* 
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Table 5.1. End-point positioning repeatability under 
payload variation 


end-point 
position in 
inches: 
mean 

(variance) 

estimation 
(robot coord.) 

x , y 

measurement 
(camera coord.) 
x , y 

measurement 
(scale) 
x , y 

joint command, 
no payload 

-139.62 , 47.68 
(0.024 , 0.042) 

14.65 , -4.81 
(0.013 , 0.016) 

7.56 , 6.63 
(- . -) 

joint command, 
55 lb payload 

-139.66 , 47.55 
(0.015 , 0.020) ' 

14.77 , -5.64 
(0.011 , 0.007) 

7.75 , 5.69 
(- . -) 

tip command, 
no payload 

-139.81 , 47.91 
(0.017 , 0.015) 

14.49 , -4.52 
(0.005 ,0.018) 

7.38 , 6.94 
(- . -) 

tip command, 
55 lb payload 

-139.96 , 48.79 
(0.025 , 0.049) 

14.50 , -4.49 
(0.017 , 0.072) 

7.40 , 6.85 
(0.045 , 0.045) 


conditions: desired position for joint command: -140.0", 48" 

desired position for tip command: 14.5", -4.55’ 

mean and variance based on 10 measurements 


! 
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Table 5.2. End-point position fluctuation under payload 
variation 


end-point position 
fluctuation in inches, 
due to 551b payload 
variation 

measurement 
(camera coordinates) 
Ax , Ay 

measurement 

(scale) 

Ax , Ay 

joint command 

0.12 , 0.83 

0.19 , 0.94 

position command 

0.01 , 0.07 

0.02 , 0.09 







5.2 


x - position (In) 


Figure 5.1. Tip-position during end-point positioning 
sequence (start inside camera work space) 
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CHAPTER VI 


CONCLUSIONS AND RECOMMENDATIONS 


A static end-point positioning control for a large two-link 
flexible manipulator was implemented and tested. 

Feedback of the end-point position of the manipulator was 
provided from a Landmark Tracking System. Sampling time and 
repeatability of this sensor were investigated under setup 
conditions. 

The sampling time was measured for illuminating a scene 6 
feet from the LTS, scanning the whole image for two landmarks and 
returning area, peak and center of gravity of both landmarks as 
ASCII characters at a Baud rate of 19200. The minimum sampling 
time was 95 ms but for normal operation a sampling time of 300 ms 
was used due to power limitations of the strobe tubes used. 

The repeatability was investigated by measuring the relative 
distance between two landmarks, 21 inches apart and 6 feet from 
the LTS. The variance of the measured distance was 0.166 mm in X- 
direction and 0.205 mm in Y-direction. 
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The controller was implemented using interrupts for joint 
control and communication with the LTS. The end-point positioning 
algorithm transforms the measured tip position error to joint angle 
error and utilizes this error to update the reference input to the 
joint controller. It was assumed that the tip-position is equivalent 
to the position of the landmark that is attached to the robot tip. 

Convergence of the positioning algorithm was demonstrated 
for the manipulator work space observed by the LTS. The tip could be 
positioned with a positioning error of less than 1.5 mm after three 
iterations when the tip was initially in the area observed by the 
camera. One or two more iterations were necessary when the tip 
was initially outside this area. 

The positioning repeatability was investigated under payload 
variation. The algorithm compensated for a deflection of 
approximately one inch, caused by attaching a 55 lb payload to the 
manipulator tip. 

Several issues should be addressed in future extensions of this 

work: 

The bottleneck of the current system is the landmark 
illumination. The strobe tube used is too weak for more rapid 
flashing and a more powerful tube did not produce sufficient 
intensity. Further testing of available flash tubes, different 
illumination techniques or the use of light sources instead of 
landmarks should be considered. 
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The communication between LTS and control system currently 
used disturbs the joint controller and limits the sampling rate of 
the position measurement. Handshaking should be implemented to 
assign different priorities to the interrupts. Assigning the highest 
priority to the timer interrupt results in a more constant joint 
controller sampling rate, since the timer interrupt would not be 
interrupted by the communication interrupt. No characters would be 
lost with handshaking since the communication would wait until the 
timer interrupt had finished. 

The communication time should also be decreased by sending 
fewer characters. Three methods to achieve this are: 1. Use a new 
version of the LTS that times the data shifting with hardware and 
does not need the relative position measurement to yield a 
satisfying resolution. In this case only the data for one landmark is 
sent. 2. Perform more processing on the LTS board. Only the position 
would have to be send when error checking and computing the 
relative position could be performed by the LTS. 3. Send binary data 
instead of ASCII. The terminal option of the control system could be 
modified to interpret non ASCII data or multiple routine for position 
measurement and terminal operation could be stored on the LTS. 

The onboard processing speed could be increased using a 
windowing algorithm that scans only a fraction of the image for a 
landmark or a faster processor could be used. 
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Implementing some of those recommendations should provide 
the necessary performance for future applications which could 
include: 1. measurement device for fast position measurements, 2. 
dynamic end-point position feedback, 3. extension to 3-dimensions 
and 4. increased work space. 


it 
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APPENDIX B 


EQUIPMENT LIST 


The following equipment and software products were added to 
the control environment for RALF, compare Huggins (1988) and 
Wilson (1985) for a listing of the original equipment. 


- 386-AT, 25 MHz CPU and math-coprocessor, 4 MByte RAM 

- MetraByte DAS-16 12 bit data acquisition board with control 

software (0.4 ms sampling time for a 8 channel scan) 

- Interface for digital-to-analog conversion with power supply. This 

Op-Amp circuit (inside DAS-16 interface box) boosts the 
output voltage from (0 .. 5) V to (-10 .. +10) V to match the 
output from DAS-16 with the existing VAX-control-system 
output. 

- Landmark Tracking System with power supply 
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Illumination unit consisting of power unit (circuit diagram in Fig. 
B.I., technical data sheet for trigger transformer in Fig. B.2.) 
and strobe tube (technical data sheet in Fig. B.3.). Strobe tubes 
and trigger transformer were purchased from 
Shokai Far East LTD. 

9 Elena Court 
Peekskill, NY 10566 
(914) 736-3500 

The parts used have the following catalog numbers: 
straight strobe tube SFT 3534T 

U-shaped strobe tube SFT 112M 

trigger transformer STR 6KS/RH1 

Borland, Turbo C ++ and Turbo Assembler 

Magna Carta Software, C Communications Toolkit 
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Figure B.2. Trigger transformer, technical data sheet, 
Shokai Far East LTD. 
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Figure B.3. Strobe tubes, techn 
East LTD. 
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APPENDIX C 


USER MANUAL 


C.l. Installation and System Start 

The following procedure is recommended to start the control 
system: 

1. Connect the blue DAS-16 interface box to the PC and turn on 
the power supply for the interface. 

2. Connect manipulator sensors and actuators to DAS-16, i.e. 
connect cable from DAS-16 interface and cable from manipulator 
interface rack with 26 pin connectors. 

3. Connect the serial line from LTS to com 1 port of PC. 

4. Turn on power for LTS and strobe unit. 

5. Turn on power for PC 

6. Go to sub-directory "c:\klaus\control4" 

7. Write zeros to the output ports of the digital-to-analog 
converter to set the voltage on the amplifiers for the hydraulic 
valves to zero (this prevents uncontrolled motion of the 
manipulator): execute program "d2atest" and type 0,0 on prompt. 
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8. Turn on power for manipulator interface rack and turn on 
valve amplifiers. 

9. Turn on hydraulic unit. 

10. Reset emergency switch, push button on interface rack 
(LED goes on). 

11. Test connections by writing a voltage to the amplifiers and 
checking if manipulator moves. Use program "d2atest" to send 
voltages (recommended test values: 300,600 for lower and upper 
joint respectively) 

12. If manipulator does not move, check 26 pin connector 
between DAS-16 interface and manipulator and switch settings of 
experimental setup on top of the manipulator interface rack 
("shake"-test). 

13. Start controller by executing program "main". 


C.2. Operation 

All control system options are chosen from menus. After start 
up the program prompts for communication rate and initial joint 
angles before displaying the main menu. 

First, the user is prompted for the Baud rate and some options 
are listed, 19200 Baud is usually used. Check switches on the LTS 
board for the Baud rate used if problems are encountered, switch 
settings and corresponding Baud rates are listed in Table C.l. 
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Second, the user is prompted to reset the LTS. (reset button on 
LTS board or turn power off and on). This starts the controller with 
a known LTS configuration. New defaults are sent to the camera 
after reset. The LTS also takes a first picture, therefore the strobe 
should flash. 

Third, the user is prompted to confirm the (desired) initial 
joint angles, the manipulator reads and displays the current position 
(which should be equivalent to the rest position if these 
instructions were followed). The displayed joint angles should be 
confirmed otherwise the program terminates. 

The main menu is displayed after the initial user input, this 
menu offers the following options: 

I Initialization menu, displays the initialization menu. 

D Debbugging menu, displays the debugging menu. 

M Move robot, this is the joint control option described in 

chapter 4.2, the robot is moved to a user specified position 
(joint angles or estimated tip-position) in a user specified 
time. Returns to main menu. 

C Camera assisted move, this is the end-point positioning 

control option described in chapter 4.2, the robot tip is 
positioned at a user specified position (in camera coordinates). 
The camera work space boundaries are displayed to guide the 
user with the position input. The algorithm prompts the user 
for confirmation when a position outside the work space is 
chosen. Returns to main menu, an error message is displayed 
when the manipulator tip couldn't be positioned. 
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R Read a2d-board, compute position, this option read the 
joint position sensor and estimates the current manipulator 
tip-position (in robot coordinates) based on rigid kinematics. 
Returns to main menu. 

P get tip Position, this option takes a picture and returns the 
measured tip-position (in camera coordinates). Returns to 
main menu. 

F repeated Fast position measurements, this option 
measures the manipulator tip-position repeatedly and displays 
the values on the screen. The current sampling time is limited 
to 300 ms. Returns to main menu. 

T read Timer, displays the time since program start. Returns to 
main menu. 

Q Quit, returns to DOS prompt. 

The initialization menu offers the following options: 

P Parameters, displays the parameter menu. 

T Terminal program, PC is used as a terminal to communicate 
with the LTS. The software commands described in chapter 
3.2.3. can be used to test the camera setup. This option should 
be used carefully since the control system does not keep track 
of LTS configuration changes. The SHOW (Sn) command can 
not be used from this option because too much data is 
transferred and the program "crashes". Returns to 
initialization menu when "<alt> + <m>" is pressed. 
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to 12 calibration points (tip-positions) inside this area. The 
positions are compared to the boundary and the user is 
prompted until all points are inside the boundary. Then the 
manipulator is moved through all calibration points and the 
Jacobian is computed from the recorded tip-positions and joint 
angles according to eq. 4.1. This Jacobian can be stored to a 
data file for later use. Returns to initialization menu. 

H Help, this option is currently not available. 

Q Quit, returns to main menu. 

The debugging menu combines several test options that were 

used during software development (all options return to debugging 

menu): 

T Trajectory computation test, tests if interrupt functions 
create errors in data computed by background functions. 

E Error code, prints out error code from timer interrupt service 
routine; this was used since -direct- debugging of interrupt 
functions is not possible. 

F read hold Flag, this option prints the value of the trajectory 
control flag to the screen. 

W Write data to disk, this option enables data storing to the 
ring buffer. 

Q Quit, returns to main menu. 
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The parameter menu displays the current parameters and 
enables parameter changes: 

A Proportional gain joint 1, sets the controller gains 

described in chapter 4.2. 

B Proportional gain joint 2, sets the controller gains 

described in chapter 4.2. 

C Min pixel value to start growing landmark, sets LTS 

threshold #0, compare Table 3.1. 

D Min pixel value to be included In landmark, threshold #1 

E Min area of landmark to be reported, threshold #2 

F Integration time, threshold #3 
G Strobe charge time, threshold #4 
H Repetition factor for integration, threshold #5 
J (data) Format, displays menu to chose data format for serial 
communication. 

K Speed, displays menu to chose Baud rate for serial 

communication. 

Q Quit, returns to initialization menu. 




90 


Table C.1. Switch setting for Baud rate selection 


Baud Rate 

SI 

S2 

S3 

S4 

S5 

S6 

S7 

S8 

57600 




n 





38400 

H 


n 

X 





28800 


n 



X 




23040 

n 

n 

n 


X 




19200 



n 

X 

X 

| 



9600 


n 


X 

n 

n 

pw 


4800 


X 

n 


X 

X 

X 


2400 


in 


X 


X 

X 

X 


Note: Switch (S) CN 

OFF 


X 

empty box 
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#if OPT I ON COWM 

pr i nt f ( M \nP . gel tip Position"}; 

pr i n t f (*'\nF . repealed. Fast position measuremenis" 
#end i f 
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#if OPTION_DEBUC /* debugging: write 'B* to screen */ 
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converts the control output to an integer 
D/A board only handles integers 
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ROUTINE 

REPORTTIPPOSITION 
K I aus fiber gf e 1 1 



DESCRIPTION 

print* information about the current camera workspace boundaries to screen 
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Fill FILE 

ads tuf f . h - function prototypes to file adsluff.c adstuff.c - routines for daslfc data acquisition board 

Klaus Obergfelt 1991 Klaus Obergfcll 1 99 1 
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DESCRIPTION 

initializes dasl6 board, sets scan liaiits (2 channel scan, channels 
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/* output both d/a channels */ 
flag • 0; 
mode - 1 6 ; 

datalO] • 2047 - oull; 
dala[l] ■ 2047 - out2j 
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SCTAlARft - set timer interrupt period in mill i seconds 
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return (0): /* normal return */ 
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convert . h - function prototypes to file convert. c convert. c - conversion routines for HALF, used by controller 

Klaus Obergf el I 1991 trajectory planner 

a**************************************************************************/ Klaus Obergf e 1 1 1 99 1 
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else /* standard cases */ 

double angl,ang2; ( 

int flag-0; argument2 - argument * argument; 
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debug. h - function prototypes for file debug. c debug. c - debugging option* for ralf control software 

Klaus Obergf e 1 1 199* * lau * Obergfell 1991 
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static void debugjaenu (void) ; 
static int test (void) ; 
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/* calculates thetal in radians */ 

si - -** (L2*sin (theta2) +1 J)+y* (l2*cos (theta2)+L I) ; 

cl - x* (L2*cos (theta2)+L l)+y* (L2*sln(theta2)+L3) ; 

norm - $qrt(sl * sl + cl * cl); 
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Klaus Obergfell. based on example from communication software 
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